/****************************************************
 ****************************************************
 ***                                              ***
 ***   Project:  YAVRS                            ***
 ***   (Yet Another Virtual Reality System)       ***
 ***   ________________________________________   ***
 ***                                              ***
 ***   File:     yavrsCamera.cpp                  ***
 ***   Authors:  Steve Conner & Thomas Marrinan   ***
 ***   Date:     09/28/2011                       ***
 ***                                              ***
 ****************************************************
 ****************************************************/

#include <cmath>
#include <fstream>
#include <iostream>
#include <string>
#include "yavrsCamera.h"

using namespace std;


/*** ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ ***/
yavrsCamera::yavrsCamera(int w, int h, const char* cfgFile) {
	// read config file
	ifstream fileIn;
	fileIn.open(cfgFile);
	
	if(!fileIn.is_open()){
		printf("Error, cannot open %s\n", cfgFile);
		exit(1);
	}
	
	double screenW_m, screenH_m;
	
	string line;
	while(getline(fileIn, line)){
		int pos;
		pos = line.find("#");
		if(pos == 0) continue; 
		if(pos > 0) line = line.substr(0, pos);
		
		pos = line.find("resolution");
		if(pos >=0){
			sscanf(line.c_str(), "resolution %d %d", &screenW_px, &screenH_px);
		}
		pos = line.find("screen_size");
		if(pos >=0){
			sscanf(line.c_str(), "screen_size %lf %lf", &screenW_m, &screenH_m);
		}
	}
	
	// calculate the length of a pixel for the display
	m_per_px = ((screenW_m/(double)screenW_px) + (screenH_m/(double)screenH_px)) / 2.0;
	
	// store width and height values for the created window (pixels and meters)
	width_px  = w;
	height_px = h;
	width_m   = (double)width_px  * m_per_px;
	height_m  = (double)height_px * m_per_px;
	
	// initialize window and cop	
	cw[0] = 0.0;
	cw[1] = 0.0;
	cw[2] = 0.0;
	
	vpn[0] =  0.0;
	vpn[1] =  0.0;
	vpn[2] = -1.0;
	viewUp[0] = 0.0;
	viewUp[1] = 1.0;
	viewUp[2] = 0.0;
	
	cop[0] = 0.0;
	cop[1] = 0.0;
	cop[2] = 1.0;
	
	// create stencil for interleaved stereo
	glViewport(0, 0, width_px, height_px);
	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity;
	glMatrixMode (GL_PROJECTION);
	glLoadIdentity();
	gluOrtho2D(0, width_px-1 , 0, height_px-1);
	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();
	
	glEnable(GL_STENCIL_TEST);
	glClearStencil(0);
	glClear(GL_STENCIL_BUFFER_BIT);
	glStencilOp (GL_REPLACE, GL_REPLACE, GL_REPLACE);
	glDisable(GL_DEPTH_TEST);
	glStencilFunc(GL_ALWAYS, 1, 1);

	glColor4f(1, 1, 1, 0);
	int i;
	for(i=0; i<height_px; i+=2){
		glLineWidth(1);
		glBegin(GL_LINES);
			glVertex2f(0, i);
			glVertex2f(width_px, i);
		glEnd();	
	}
	glStencilOp (GL_KEEP, GL_KEEP, GL_KEEP);
	glEnable(GL_DEPTH_TEST);
	
	// initialize stereo rendering mode
	mode = NONE;
    
    // initialize interaction mode
	iMode = FIRST_PERSON;
    
	// initialize the render function
	renderFunction = NULL; 
}

/*** ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ ***/
void yavrsCamera::resize(int w, int h) {
	// store width and height values for the resized window (pixels and meters)
	width_px  = w;
	height_px = h;
	width_m   = (double)width_px  * m_per_px;
	height_m  = (double)height_px * m_per_px;
	
	// recreate stencil for INTERLEAVED stereo
	glViewport(0, 0, width_px, height_px);
	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity;
	glMatrixMode (GL_PROJECTION);
	glLoadIdentity();
	gluOrtho2D(0, width_px-1 , 0, height_px-1);
	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();
	
	glEnable(GL_STENCIL_TEST);
	glClearStencil(0);
	glClear(GL_STENCIL_BUFFER_BIT);
	glStencilOp (GL_REPLACE, GL_REPLACE, GL_REPLACE);
	glDisable(GL_DEPTH_TEST);
	glStencilFunc(GL_ALWAYS, 1, 1);
	
	glColor4f(1, 1, 1, 0);
	int i;
	for(i=0; i<height_px; i+=2){
		glLineWidth(1);
		glBegin(GL_LINES);
		glVertex2f(0, i);
		glVertex2f(width_px, i);
		glEnd();	
	}
	glStencilOp (GL_KEEP, GL_KEEP, GL_KEEP);
	glEnable(GL_DEPTH_TEST);
	
	updatePerspective();
}

/*** ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ ***/
void yavrsCamera::renderFunc(void (*f)()) {
	renderFunction = f;
}

/*** ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ ***/
void yavrsCamera::renderScene() {
	switch (mode) {
		case NONE:
			// draw scene from cop
			updatePerspective();
			renderFunction();
			break;
		case LEFT_EYE:
			// draw scene from left eye
			updatePerspective(LEFT_EYE);
			renderFunction();
			break;
		case RIGHT_EYE:
			// draw scene from right eye
			updatePerspective(RIGHT_EYE);
			renderFunction();
			break;
		case INTERLEAVED:
			// draw left eye
			glStencilFunc(GL_NOTEQUAL, 1, 1); // draws if stencil != 1
			updatePerspective(LEFT_EYE);
			renderFunction();
			// draw right eye
			glStencilFunc(GL_EQUAL, 1, 1); // draws if stencil == 1
			updatePerspective(RIGHT_EYE);
			renderFunction();
			break;
		case SIDE_BY_SIDE:
			// draw left eye
			glViewport(0, 0, width_px/2, height_px);
			updatePerspective(LEFT_EYE);
			renderFunction();
			// draw right eye
			glViewport(width_px/2, 0, width_px/2, height_px);
			updatePerspective(RIGHT_EYE);
			renderFunction();
	}
}

/*** ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ ***/
void yavrsCamera::setView(double winCen[], double camDir[], double camUp[], double projCen[]) {
	cw[0]     = winCen[0];  cw[1]     = winCen[1];  cw[2]     = winCen[2];
	vpn[0]    = camDir[0];  vpn[1]    = camDir[1];  vpn[2]    = camDir[2];
	viewUp[0] = camUp[0];   viewUp[1] = camUp[1];   viewUp[2] = camUp[2];
	cop[0]    = projCen[0]; cop[1]    = projCen[1]; cop[2]    = projCen[2];
}

/*** ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ ***/
void yavrsCamera::setStereoMode(stereo_mode m) {
	mode = m;
	if(mode == INTERLEAVED){
		glEnable(GL_STENCIL_TEST);
	}
	else{
		glDisable(GL_STENCIL_TEST);
	}
	if(mode != SIDE_BY_SIDE){
		glViewport(0, 0, width_px, height_px);
	}
}

/*** ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ ***/
void yavrsCamera::setInteractionMode(interaction_mode m) {
	iMode = m;
}

/*** ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ ***/
void yavrsCamera::setCOP(double x, double y, double z) {
	cop[0] = x;
	cop[1] = y;
	cop[2] = z;
	
	updatePerspective();
}

/*** ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ ***/
void yavrsCamera::setOrientation(double x, double y, double z, double w) {
	
	updatePerspective();
}

/*** ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ ***/
void yavrsCamera::translateCOP(double x, double y, double z) {
	cop[0] += x;
	cop[1] += y;
	cop[2] += z;
	
	updatePerspective();
}

/*** ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ ***/
void yavrsCamera::translateCamera(double x, double y, double z) {
	double tForward[3], tUp[3], tRight[3];
	
	// tForward = vpn
	tForward[0] = vpn[0];
	tForward[1] = vpn[1];
	tForward[2] = vpn[2];
	
	// tUp = viewUp
	tUp[0] = viewUp[0];
	tUp[1] = viewUp[1];
	tUp[2] = viewUp[2];
	
	// tRight vector = tForward X tUp
	tRight[0] = tForward[1]*tUp[2] - tForward[2]*tUp[1];
	tRight[1] = tForward[2]*tUp[0] - tForward[0]*tUp[2];
	tRight[2] = tForward[0]*tUp[1] - tForward[1]*tUp[0];
	
	// scale vectors by translation values
	int i;
	for(i=0; i<3; i++){
		tRight[i] *= x;
		tUp[i] *= y;
		tForward[i] *= z;
	}
	
	// translate the center of the window
	cw[0] += tRight[0] + tUp[0] + tForward[0];
	cw[1] += tRight[1] + tUp[1] + tForward[1];
	cw[2] += tRight[2] + tUp[2] + tForward[2];
	
	updatePerspective();
}

/*** ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ ***/
void yavrsCamera::rotateCamera(double axis[], double deg) {
	double abc[3];
	getCameraPosition(abc);
	
	double x = cw[0];
	double y = cw[1];
	double z = cw[2];
	double a = abc[0];
	double b = abc[1];
	double c = abc[2];
	double u = axis[0];
	double v = axis[1];
	double w = axis[2];
	double T = deg * PI/180.0;
	
	// rotate center of window about the camera position
	cw[0] = (a*(v*v+w*w)-u*(b*v+c*w-u*x-v*y-w*z))*(1-cos(T))+x*cos(T)+(-c*v+b*w-w*y+v*z)*sin(T);
	cw[1] = (b*(u*u+w*w)-v*(a*u+c*w-u*x-v*y-w*z))*(1-cos(T))+y*cos(T)+( c*u-a*w+w*x-u*z)*sin(T);
	cw[2] = (c*(u*u+v*v)-w*(a*u+b*v-u*x-v*y-w*z))*(1-cos(T))+z*cos(T)+(-b*u+a*v-v*x+u*y)*sin(T);
	
	// rotate viewUp vector about the origin
	a = 0.0;
	b = 0.0;
	c = 0.0;
	x = viewUp[0];
	y = viewUp[1];
	z = viewUp[2];
	
	viewUp[0] = (a*(v*v+w*w)-u*(b*v+c*w-u*x-v*y-w*z))*(1-cos(T))+x*cos(T)+(-c*v+b*w-w*y+v*z)*sin(T);
	viewUp[1] = (b*(u*u+w*w)-v*(a*u+c*w-u*x-v*y-w*z))*(1-cos(T))+y*cos(T)+( c*u-a*w+w*x-u*z)*sin(T);
	viewUp[2] = (c*(u*u+v*v)-w*(a*u+b*v-u*x-v*y-w*z))*(1-cos(T))+z*cos(T)+(-b*u+a*v-v*x+u*y)*sin(T);
	
	// rotate vpn vector about the origin
	x = vpn[0];
	y = vpn[1];
	z = vpn[2];
	
	vpn[0] = (a*(v*v+w*w)-u*(b*v+c*w-u*x-v*y-w*z))*(1-cos(T))+x*cos(T)+(-c*v+b*w-w*y+v*z)*sin(T);
	vpn[1] = (b*(u*u+w*w)-v*(a*u+c*w-u*x-v*y-w*z))*(1-cos(T))+y*cos(T)+( c*u-a*w+w*x-u*z)*sin(T);
	vpn[2] = (c*(u*u+v*v)-w*(a*u+b*v-u*x-v*y-w*z))*(1-cos(T))+z*cos(T)+(-b*u+a*v-v*x+u*y)*sin(T);
	
	updatePerspective();
}

/*** ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ ***/
void yavrsCamera::lookUp(double deg) {
	// viewRight vector = vpn X viewUp
	double viewRight[3];
	viewRight[0] = vpn[1]*viewUp[2] - vpn[2]*viewUp[1];
	viewRight[1] = vpn[2]*viewUp[0] - vpn[0]*viewUp[2];
	viewRight[2] = vpn[0]*viewUp[1] - vpn[1]*viewUp[0];
	
    if(iMode == FREE_ROTATE){
        rotateCamera(viewRight, deg);
    }
    else{
        if(((acos(vpn[1])*(180.0/PI)) > (deg+20.0)) && ((acos(-vpn[1])*(180.0/PI)) > (-deg+20.0))){
            rotateCamera(viewRight, deg);
        }
    }
}

/*** ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ ***/
void yavrsCamera::lookDown(double deg) {
	// viewRight vector = vpn X viewUp
	double viewRight[3];
	viewRight[0] = vpn[1]*viewUp[2] - vpn[2]*viewUp[1];
	viewRight[1] = vpn[2]*viewUp[0] - vpn[0]*viewUp[2];
	viewRight[2] = vpn[0]*viewUp[1] - vpn[1]*viewUp[0];
	
    if(iMode == FREE_ROTATE){
        rotateCamera(viewRight, -deg);
    }
    else{
        if(((acos(vpn[1])*(180.0/PI)) > (-deg+20.0)) && ((acos(-vpn[1])*(180.0/PI)) > (deg+20.0))){
            rotateCamera(viewRight, -deg);
        }
    }
}

/*** ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ ***/
void yavrsCamera::lookLeft(double deg) {
	if(iMode == FREE_ROTATE){
        rotateCamera(viewUp, deg);
    }
    else{
        double yUp[3];
        yUp[0] = 0.0;
        yUp[1] = 1.0;
        yUp[2] = 0.0;
        rotateCamera(yUp, deg);
    }
}

/*** ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ ***/
void yavrsCamera::lookRight(double deg) {
	if(iMode == FREE_ROTATE){
        rotateCamera(viewUp, -deg);
    }
    else{
        double yUp[3];
        yUp[0] = 0.0;
        yUp[1] = 1.0;
        yUp[2] = 0.0;
        rotateCamera(yUp, -deg);
    }
}

/*** ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ ***/
void yavrsCamera::lookClockWise(double deg) {
	if(iMode == FREE_ROTATE){
        rotateCamera(vpn, deg);
    }
}

/*** ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ ***/
void yavrsCamera::lookCounterClockWise(double deg) {
	if(iMode == FREE_ROTATE){
        rotateCamera(vpn, -deg);
    }
}

/*** ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ ***/
void yavrsCamera::vrc2world(double vrcPt[], double worldPt[]) {
	double tForward[3], tUp[3], tRight[3];
	
	// tForward = vpn
	tForward[0] = vpn[0];
	tForward[1] = vpn[1];
	tForward[2] = vpn[2];
	
	// tUp = viewUp
	tUp[0] = viewUp[0];
	tUp[1] = viewUp[1];
	tUp[2] = viewUp[2];
	
	// tRight vector = tForward X tUp
	tRight[0] = tForward[1]*tUp[2] - tForward[2]*tUp[1];
	tRight[1] = tForward[2]*tUp[0] - tForward[0]*tUp[2];
	tRight[2] = tForward[0]*tUp[1] - tForward[1]*tUp[0];
	
	// scale vectors by vrc point
	int i;
	for(i=0; i<3; i++){
		tRight[i] *= vrcPt[0];
		tUp[i] *= vrcPt[1];
		tForward[i] *= -vrcPt[2];
	}
	
	// calculate cop in world coordinates
	worldPt[0] = cw[0] + tRight[0] + tUp[0] + tForward[0];
	worldPt[1] = cw[1] + tRight[1] + tUp[1] + tForward[1];
	worldPt[2] = cw[2] + tRight[2] + tUp[2] + tForward[2];
}

/*** ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ ***/
void yavrsCamera::getScreenSize(int size[]) {
	size[0] = width_px;
	size[1] = height_px;
}

/*** ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ ***/
void yavrsCamera::getCameraPosition(double cop_wc[]) {
	vrc2world(cop, cop_wc);
}


/*** ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ ***/
void yavrsCamera::getWindowDirection(double windowDir[]) {
	windowDir[0] = vpn[0];
	windowDir[1] = vpn[1];
	windowDir[2] = vpn[2];
}

/*** ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ ***/
void yavrsCamera::getWindowUp(double windowUp[]) {
	windowUp[0] = viewUp[0];
	windowUp[1] = viewUp[1];
	windowUp[2] = viewUp[2];
}

/*** ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ ***/
void yavrsCamera::getWindowCenter(double windowCenter[]) {
	windowCenter[0] = cw[0];
	windowCenter[1] = cw[1];
	windowCenter[2] = cw[2];
}

/*** ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ ***/
void yavrsCamera::getWindowRT(double windowRT[]) {
	// viewRight vector = vpn X viewUp
	double viewRight[3];
	viewRight[0] = vpn[1]*viewUp[2] - vpn[2]*viewUp[1];
	viewRight[1] = vpn[2]*viewUp[0] - vpn[0]*viewUp[2];
	viewRight[2] = vpn[0]*viewUp[1] - vpn[1]*viewUp[0];
	
	windowRT[0] = cw[0] + (viewRight[0]*width_m/2.0) + (viewUp[0]*height_m/2.0);
	windowRT[1] = cw[1] + (viewRight[1]*width_m/2.0) + (viewUp[1]*height_m/2.0);
	windowRT[2] = cw[2] + (viewRight[2]*width_m/2.0) + (viewUp[2]*height_m/2.0);
}

/*** ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ ***/
void yavrsCamera::getWindowLT(double windowLT[]) {
	// viewRight vector = vpn X viewUp
	double viewRight[3];
	viewRight[0] = vpn[1]*viewUp[2] - vpn[2]*viewUp[1];
	viewRight[1] = vpn[2]*viewUp[0] - vpn[0]*viewUp[2];
	viewRight[2] = vpn[0]*viewUp[1] - vpn[1]*viewUp[0];
	
	windowLT[0] = cw[0] - (viewRight[0]*width_m/2.0) + (viewUp[0]*height_m/2.0);
	windowLT[1] = cw[1] - (viewRight[1]*width_m/2.0) + (viewUp[1]*height_m/2.0);
	windowLT[2] = cw[2] - (viewRight[2]*width_m/2.0) + (viewUp[2]*height_m/2.0);
}

/*** ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ ***/
void yavrsCamera::getWindowLB(double windowLB[]) {
	// viewRight vector = vpn X viewUp
	double viewRight[3];
	viewRight[0] = vpn[1]*viewUp[2] - vpn[2]*viewUp[1];
	viewRight[1] = vpn[2]*viewUp[0] - vpn[0]*viewUp[2];
	viewRight[2] = vpn[0]*viewUp[1] - vpn[1]*viewUp[0];
	
	windowLB[0] = cw[0] - (viewRight[0]*width_m/2.0) - (viewUp[0]*height_m/2.0);
	windowLB[1] = cw[1] - (viewRight[1]*width_m/2.0) - (viewUp[1]*height_m/2.0);
	windowLB[2] = cw[2] - (viewRight[2]*width_m/2.0) - (viewUp[2]*height_m/2.0);
}

/*** ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ ***/
void yavrsCamera::getWindowRB(double windowRB[]) {
	// viewRight vector = vpn X viewUp
	double viewRight[3];
	viewRight[0] = vpn[1]*viewUp[2] - vpn[2]*viewUp[1];
	viewRight[1] = vpn[2]*viewUp[0] - vpn[0]*viewUp[2];
	viewRight[2] = vpn[0]*viewUp[1] - vpn[1]*viewUp[0];
	
	windowRB[0] = cw[0] + (viewRight[0]*width_m/2.0) - (viewUp[0]*height_m/2.0);
	windowRB[1] = cw[1] + (viewRight[1]*width_m/2.0) - (viewUp[1]*height_m/2.0);
	windowRB[2] = cw[2] + (viewRight[2]*width_m/2.0) - (viewUp[2]*height_m/2.0);
}

/*** ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ ***/
void yavrsCamera::updatePerspective(stereo_mode m) {
	// add 646, 318 for Marrinan Laptop to EVL VR
	int screen_pos_x = glutGet(GLUT_WINDOW_X) + 646;
	int screen_pos_y = glutGet(GLUT_WINDOW_Y) + 318;
	
	double offsetX = (double)(screen_pos_x+(width_px/2) -(screenW_px/2)) * m_per_px;
	double offsetY = (double)((screenH_px-screen_pos_y)-(height_px/2)-(screenH_px/2)) * m_per_px;
	
	double viewRight[3];
	viewRight[0] = vpn[1]*viewUp[2] - vpn[2]*viewUp[1];
	viewRight[1] = vpn[2]*viewUp[0] - vpn[0]*viewUp[2];
	viewRight[2] = vpn[0]*viewUp[1] - vpn[1]*viewUp[0];
	
	double cop_eye[3];
	cop_eye[0] = cop[0]; cop_eye[1] = cop[1]; cop_eye[2] = cop[2];
	
	if(m == LEFT_EYE){
		cop_eye[0] -= 0.032;
	}
	if(m == RIGHT_EYE){
		cop_eye[0] += 0.032;
	}
	
	double near   = 0.05;
	double far    = 600.0;
	double left   = (offsetX-(width_m/2.0)  - cop_eye[0]) * (near/cop_eye[2]);
	double right  = (offsetX+(width_m/2.0)  - cop_eye[0]) * (near/cop_eye[2]);
	double bottom = (offsetY-(height_m/2.0) - cop_eye[1]) * (near/cop_eye[2]);
	double top    = (offsetY+(height_m/2.0) - cop_eye[1]) * (near/cop_eye[2]);
	
	double eye_wc[3];
	vrc2world(cop_eye, eye_wc);
	
	double viewFoc[3];
	viewFoc[0] = eye_wc[0] + vpn[0];
	viewFoc[1] = eye_wc[1] + vpn[1];
	viewFoc[2] = eye_wc[2] + vpn[2];
	
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	glFrustum(left, right, bottom, top, near, far);
	
	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();
	gluLookAt(eye_wc[0], eye_wc[1], eye_wc[2], viewFoc[0], viewFoc[1], viewFoc[2], viewUp[0], viewUp[1], viewUp[2]);
}

stereo_mode yavrsCamera::getStereoMode() {
	return mode;
}



